/*  -------------------------- (C) COPYRIGHT 2020 Fortiortech ShenZhen ---------------------------*/
/*  File Name      : AddFunction.c
/*  Author         : Fortiortech  Appliction Team
/*  Version        : V1.0
/*  Date           : 2020-08-21
/*  Description    : This file contains  function used for Motor Control.
/*  ----------------------------------------------------------------------------------------------*/
/*                                     All Rights Reserved
/*  ----------------------------------------------------------------------------------------------*/

#include "MyProject.h"

/* Private variables ---------------------------------------------------------*/
/*电机保护相关*/
FaultStateType mcFaultSource;       //错误状态标志位
FaultVarible xdata mcFaultDect;     //错误计数
//ProtectVarible xdata mcProtectTime; //保护标志位

/*电机控制变量*/
bool idata FR;
FOCCTRL xdata mcFocCtrl;
MCRAMP idata MotorSpeed;
//BL_150 xdata BLDC150;
/*ONOFF测试*/
ONVarible ONOFFTest;

 PWMINPUTCAL idata mcPwmInput;
 
 
 
 /*  -------------------------------------------------------------------------------------------------
    Function Name  : Abs_F32
    Description    : 对变量取32位的绝对值
    Date           : 2020-04-10
    Parameter      : value: [输入/出]
    ------------------------------------------------------------------------------------------------- */
uint32 Abs_F32(int32 value)
{
	if (value < 0)
	{
		return (-value);
	}
	else
	{
		return (value);
	}
}
 
 
/*  -------------------------------------------------------------------------------------------------
    Function Name  : StarRampDealwith
    Description    :
    Date           : 2020-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void StarRampDealwith(void)
{

    uint8 ANGLE_MTemp, ANGLE_DTemp;

    if ((BLDC.BLDCSetFlag == 2) && (mcState == mcRun))
    {
        /*  -------------------------------------------------------------------------------------------------
                屏蔽续流时间（角度）设置
                -------------------------------------------------------------------------------------------------*/
        ANGLE_MTemp = (uint8)(TIM1_CR1 & 0x7F); //读取当前的续流屏蔽和延迟换相
        ANGLE_DTemp = (uint8)(TIM1_CR2 & 0x7F);

        if (ANGLE_MASK_R(Motor_Mask_Angle) > ANGLE_MTemp) //如果目标的续流屏蔽角度 > 当前续流屏蔽角度
        {
            TIM1_CR1 = (TIM1_CR1 & (0x80)) | (ANGLE_MTemp + 1); //当前续流屏蔽角度+1
        }
        else if (ANGLE_MASK_R(Motor_Mask_Angle) < ANGLE_MTemp) //如果目标的续流屏蔽角度 < 当前续流屏蔽角度
        {
            TIM1_CR1 = (TIM1_CR1 & (0x80)) | (ANGLE_MTemp - 1); //当前续流屏蔽角度-1
        }

        /*  -------------------------------------------------------------------------------------------------
            检测到过零点后的延迟换相时间（角度）设置
            -------------------------------------------------------------------------------------------------*/

        {
            if ((ANGLE_DELAY_R((30 - Motor_Advance_Angle)) + 0x01) > ANGLE_DTemp) //如果目标的延迟换相蔽角度 > 当前延迟换相角度
            {
                TIM1_CR2 = (TIM1_CR2 & (0x80)) | (ANGLE_DTemp + 1); //当前延迟换相角度+1
            }
            else if ((ANGLE_DELAY_R((30 - Motor_Advance_Angle)) + 0x01) < ANGLE_DTemp) //如果目标的延迟换相蔽角度 < 当前延迟换相角度
            {
                TIM1_CR2 = (TIM1_CR2 & (0x80)) | (ANGLE_DTemp - 1); //当前延迟换相角度-1
            }
        }

    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : VSPSample
    Description    : VSP采样
    Date           : 2020-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void VSPSample(void)
{

	static uint16 MotorSpeedVSP;
	static uint16 MotorSpeedVSP_LSB;
    /*****VREF的采样获取值并滤波******/
	LPF_MDU(ADC7_DR, 30, MotorSpeedVSP, MotorSpeedVSP_LSB);   //低通滤波

	if ((MotorSpeedVSP > ONPWMDuty))  //在ONPwmDuty-OFFPwmDutyHigh之间，电机有转速运行
	{
		MotorSpeed.FlagONOFF = 1;
	}
	else if ((MotorSpeedVSP < OFFPWMDuty)) //电机停机
	{
		MotorSpeed.FlagONOFF = 0;
	}

    /* -----转速曲线计算----- */
	if (MotorSpeed.FlagONOFF == 1)                        //
	{
		if (MotorSpeedVSP <= MINPWMDuty)   //最小转速运行
		{
			MotorSpeed.TargetValue = MOTOR_LOOP_MIN;
		}
		else if (MotorSpeedVSP < MAXPWMDuty) //调速
		{
			MotorSpeed.TargetValue = MOTOR_LOOP_MIN + LOOP_K * (MotorSpeedVSP - MINPWMDuty);
		}
		else                    //最大转速运行
		{
			MotorSpeed.TargetValue = MOTOR_LOOP_MAX;
		}
	}
	else
	{
		MotorSpeed.TargetValue = 0;
	}
	


}


/* -------------------------------------------------------------------------------------------------
    Function Name  : PWMInputCapture
    Description    : 输入PWM处理
    Date           : 2020-04-10
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void PWMInputCapture(void)
{
	uint16 MotorSpeedVSP;

	if (mcPwmInput.PwmUpdateFlag == 1) // 有新的duty更新
	{
		if ((Abs_F32(mcPwmInput.PwmCompare - mcPwmInput.PwmCompareOld) < 500) // 两次比较值相近，减少读错率
			&& (Abs_F32(mcPwmInput.PwmArrOld - mcPwmInput.PwmArr) < 500)      // 两次周期值相近，减少读错率
			&& (mcPwmInput.PwmDivFlag == 0) && (mcPwmInput.PwmDealFlag == 0))
		{
			mcPwmInput.PwmFlag = 1;                           // PwmFlag置1期间，不读取TIM3__DR和TIM3__ARR;，防止出错
			mcPwmInput.PwmCompareUpdate = (mcPwmInput.PwmCompare >> 1);// 对其乘以32768
			mcPwmInput.PwmArrUpdate = mcPwmInput.PwmArr;
			mcPwmInput.PwmDivFlag = 1;                           // 启动除法
			mcPwmInput.PwmFlag = 0;
			mcPwmInput.PwmDealFlag = 1;                           // 更新PWM后必须进过一次PWM判断才能重新进入
		}

		if ((mcPwmInput.PwmDivFlag == 0) && (mcPwmInput.PwmDealFlag == 1))
		{
			mcPwmInput.PwmDealFlag = 0;
            /***速度随PWM增大而增大***/
#if (PWMDUTY_Choose == NegaPWMDUTY)
			{
				MotorSpeedVSP = 32768 - mcPwmInput.PwmDuty;
			}
            /***速度随PWM增大而减小***/
#elif (PWMDUTY_Choose == PosiPWMDUTY)
			{
				MotorSpeedVSP = mcPwmInput.PwmDuty;
			}
#endif

			if ((MotorSpeedVSP > ONPWMDuty) && (MotorSpeedVSP <= (OFFPWMDutyHigh + 1)))
			{
				MotorSpeed.FlagONOFF = 1;
			}
			else if ((MotorSpeedVSP < OFFPWMDuty) || (MotorSpeedVSP > OFFPWMDutyHigh))
			{
				MotorSpeed.FlagONOFF = 0;
			}
            //转速曲线计算
			if (MotorSpeed.FlagONOFF == 1)
			{
				SetBit(TIM4_CR1, T4EN);
#if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
				{
					if (MotorSpeedVSP <= MINPWMDuty)   //最小转速运行
					{
						MotorSpeed.TargetValue = MOTOR_LOOP_MIN;
					}
					else if (MotorSpeedVSP < MAXPWMDuty) //调速
					{
						MotorSpeed.TargetValue = MOTOR_LOOP_MIN + LOOP_K * (MotorSpeedVSP - MINPWMDuty);
					}
					else                    //最大转速运行
					{
						MotorSpeed.TargetValue = MOTOR_LOOP_MAX;
					}
				}
#elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
				{
					if (MotorSpeedVSP <= MINPWMDuty)   //最小转速运行
					{
						MotorSpeed.TargetValue = Motor_Max_Power;
					}
					else if (MotorSpeedVSP < MAXPWMDuty) //调速
					{
						MotorSpeed.TargetValue = Motor_Min_Power + POWER_K * (MotorSpeedVSP - MINPWMDuty);
					}
					else             //最大转速运行
					{
						MotorSpeed.TargetValue = Motor_Max_Power;
					}
				}
#elif (Motor_Speed_Control_Mode == TORQUE_LOOP_CONTROL)
				{
					if (MotorSpeedVSP <= MINPWMDuty)   //最小转矩
					{
						MotorSpeed.TargetValue = SOUTMIN;
					}
					else if (MotorSpeedVSP < MAXPWMDuty) //调转矩
					{
						MotorSpeed.TargetValue = SOUTMIN + CURRENT_K * (MotorSpeedVSP - MINPWMDuty);
					}
					else                    //最大转矩
					{
						MotorSpeed.TargetValue = SOUTMAX;
					}
				}
#endif
			}
			else
			{

				ClrBit(TIM4_CR1, T4EN);
				FOC_KFG = 0;
				TIM4__ARR = 0;
				TIM4__DR = 0;
				TIM4__CNTR = 0;
				MotorSpeed.TargetValue = 0;
			}
		}

		mcPwmInput.PwmUpdateFlag = 0;
		mcPwmInput.PwmCompareOld = mcPwmInput.PwmCompare;//将此次比较值赋值给上次比较值
		mcPwmInput.PwmArrOld = mcPwmInput.PwmArr;//将此次周期值赋值给上次周期值


	}

}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : APP_DIV
    Description    : 将所有用到除法的地方，放在同一个中断，以避免中断串扰
    Date           : 2020-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void APP_DIV(void)
{
	if (mcPwmInput.PwmDivFlag == 1)
	{
		DivQ_L_MDU(mcPwmInput.PwmCompareUpdate, 0x0000, mcPwmInput.PwmArrUpdate, mcPwmInput.PwmDuty);
		mcPwmInput.PwmDivFlag = 0;
	}
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : SCLKInputCapture
    Description    : 输入PWM频率处理
    Date           : 2020-04-10
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void SCLKInputCapture(void)
{
    if (mcPwmInput.PwmUpdateFlag == 1) // 有新的duty更新
    {
        if (Abs_F32(mcPwmInput.PwmArrOld - mcPwmInput.PwmArr) < 100)  // 两次周期值相近，减少读错率
        {
            DivQ_L_MDU(TIM3_FreH, TIM3_FreL, mcPwmInput.PwmArr, mcPwmInput.PwmSclk);
            
            if ((mcPwmInput.PwmSclk > ONPWMSclk) && (mcPwmInput.PwmSclk <= (OFFPWMSclkHigh + 1)))
            {
                MotorSpeed.FlagONOFF = 1;
            }
            else if (mcPwmInput.PwmSclk < OFFPWMSclk)
            {
                MotorSpeed.FlagONOFF = 0;
            }
            
            //转速曲线计算
            if (MotorSpeed.FlagONOFF == 1)
            {
                if (mcPwmInput.PwmSclk <= MINPWMSclk)
                {
                    MotorSpeed.TargetValue = MOTOR_LOOP_MIN;
                }
                else if (mcPwmInput.PwmSclk < MAXPWMSclk)
                {
                    MotorSpeed.TargetValue = SCLK_SPEED_K * mcPwmInput.PwmSclk;
                }
                else                    //最大转速运行
                {
                    MotorSpeed.TargetValue = MOTOR_LOOP_MAX;
                }
            }
            else
            {
                MotorSpeed.TargetValue = 0;
            }
        }
        
        mcPwmInput.PwmUpdateFlag = 0;
        mcPwmInput.PwmArrOld     = mcPwmInput.PwmArr;//将此次周期值赋值给上次周期值
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : ONOFF_Starttest
    Description    : 启动测试
    Date           : 2020-04-10
    Parameter      : h_test: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void ONOFF_Starttest(void)
{
    if (ONOFFTest.ONOFF_Flag == 1)
    {
        ONOFFTest.ON_Count++;
        
        if (ONOFFTest.ON_Count > StartON_Time)
        {
            ONOFFTest.ON_Count        = 0;
            ONOFFTest.ONOFF_Times++;
            ONOFFTest.ONOFF_Flag      = 0;
            MotorSpeed.FlagONOFF   = 0;
            MotorSpeed.TargetValue = 0;
        }
    }
    else
    {
        if (mcState != mcFault)
        {
            ONOFFTest.OFF_Count++;
            
            if (ONOFFTest.OFF_Count > StartOFF_Time)
            {
                ONOFFTest.OFF_Count       = 0;
                ONOFFTest.ONOFF_Flag      = 1;
                MotorSpeed.FlagONOFF   = 1;
                MotorSpeed.TargetValue = MOTOR_LOOP_MIN;
            }
        }
    }
}


/*----- 获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE,KEYSCANMODE)的目标值修改 -----*/
void TargetSpeed_Colletion()
{
	
#if (SPEED_MODE == PWMMODE)
	{
		PWMInputCapture();
		APP_DIV();
	}
#elif (SPEED_MODE == NONEMODE)
	{
		MotorSpeed.FlagONOFF = 1;                /* 启动电机 */
		MotorSpeed.TargetValue = MOTOR_LOOP_MAX;   /* 设置目标转速 */
	}
#elif (SPEED_MODE == SREFMODE)
	{
		VSPSample();
	}
#elif (SPEED_MODE == SCLKMODE)
	{
		SCLKInputCapture();
	}
#elif (SPEED_MODE == UARTMODE)  //简单串口调速，没有加起始位、检验位等
	{
		if (Uart.SpeedRef == 0)
		{
			MotorSpeed.FlagONOFF = 0;                                  //关机
			MotorSpeed.TargetValue = 0;
		}
		else
		{
			MotorSpeed.FlagONOFF = 1;                                          //启动标志
			MotorSpeed.TargetValue = S_Value(Uart.SpeedRef);     //固定转速赋值
		}
	}
#elif (SPEED_MODE == KEYMODE)
	{
		KeyScan();
	}
#elif (SPEED_MODE == ONOFFTEST)
	{
		ONOFF_Starttest();
	}
#endif
	
}

/**
 * @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
 *               该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
 * @date         2022-09-13
 */
void TickCycle_1ms(void)
{

	/*母线电压保护作用
	 * ADC数据读取*/
	LPF_MDU(ADC14_DR, 40, mcFocCtrl.mcDcbusFlt, mcFocCtrl.mcDcbusFlt_LSB); /*母线电压读取，过欠压保护使用 */
	
	mcFocCtrl.mcIbusADC = ADC3_DR; /* RC滤波后的母线电流读取*/

	if (mcFocCtrl.mcIbusADC > mcCurOffset.Iw_busOffset) /*减去偏置电压*/
	{
		mcFocCtrl.mcIbusADC = mcFocCtrl.mcIbusADC - mcCurOffset.Iw_busOffset;
	}
	else
	{
		mcFocCtrl.mcIbusADC = 0;
	}

	LPF_MDU(mcFocCtrl.mcIbusADC, 63, mcFocCtrl.mcIbusFlt, mcFocCtrl.mcIbusFlt_LSB); /*对电流进行滤波*/
	
	/*----- 获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE,KEYSCANMODE)的目标值修改 -----*/
	TargetSpeed_Colletion();
	
	/*电机保护函数*/
	Fault_Detection(); /*故障检测和保护*/
	
	/*计数值操作  1ms计数 */
	if (ONOFFTest.PowerOFF_Count)
	{ ONOFFTest.PowerOFF_Count--; }

	if (mcFocCtrl.State_Count)
	{ mcFocCtrl.State_Count--; }

	/*延迟换相角度+续流屏蔽角度 调整*/
	StarRampDealwith();

	
	

	

}







